#include "HandModel.h"
#include <math.h>
#include <stdint.h>

// Dinh nghia 1 class Hmfinger
HmFinger::HmFinger()
{

}

HmFinger::~HmFinger()
{

}

void HmFinger::setDefaultParameter()
{
	setFingerParameter(0,	9.5f,	9.0f,	40.0f);
	setFingerParameter(1,	8.5f,	8.0f,	28.0f);
	setFingerParameter(2,	7.5f,	7.0f,	20.0f);
}

void HmFinger::setFingerParameter(int bone_index,float radious1,float radious2,float length)
{
	bone[bone_index].length = length;
	bone[bone_index].radius1 = radious1;
	bone[bone_index].radius2 = radious2;
}

HandModel::HandModel(void)
{
	lastTime = glfwGetTime();
	frame = 0;
	
	handScale = PIXEL_PER_MM;
	wirst_rotation=Eigen::Vector3f(0,0,0);
	wirst_position[0]=0;wirst_position[1]=0;wirst_position[2]=-1000;
	handDOFs = new double(23);

	finger_1.setDefaultParameter();
	finger_2.setDefaultParameter();
	finger_2.bone[0].length = 45;
	finger_2.bone[1].length = 30;
	finger_3.setDefaultParameter();
	
	finger_thumb.setFingerParameter(0,	13,	11,	40);
	finger_thumb.setFingerParameter(1,	11,	10,	35);
	finger_thumb.setFingerParameter(2,	10,	9,	25);
	finger_thumb.type = HM_FINGER_THUMB;

	finger_little.setFingerParameter(0,	7.5f,	7.3f,	33);
	finger_little.setFingerParameter(1,	7.3f,	7.0f,	20);
	finger_little.setFingerParameter(2,	7.0f,	6.8f,	20);
	finger_little.type = HM_FINGER_LITTLE;

	finger_1.type = HM_FINGER_1;
	finger_2.type = HM_FINGER_2;
	finger_3.type = HM_FINGER_3;

	createCircleTable(&sint,&cost,N_SLICES);	
}

void HandModel::generateVertexBuffers()
{
	generateCylinderBuffer();
	generateSphereBuffer();
	generateFingersBuffer();
}

HandModel::~HandModel(void)
{
}

void HandModel::drawHandBone(handBone bone,double rotation)
{
	glRotated(rotation,1,0,0); //rotate x : Ve bac tu do thu nhat (Goc xoay theo phuong X)
	glCallList(bone.vertexDisplayList);
	glTranslatef(0,bone.length,0); // Tinh tien theo chieu Y 1 doan = do dai dot ngon tay
	drawSphere(bone.radius2); // Ve khop cau
}

// Ve ca ngon tay
void HandModel::drawFinger(HmFinger hm_finger){
	int offset = NUM_DOF;
	switch(hm_finger.type){
	case HM_FINGER_THUMB:
		offset = OFFSET_THUMB;
		glRotated(handDOFs[offset+1],0,1,0);//rotate y 
		glRotated(handDOFs[offset],0,0,1);//rotate z
		glTranslatef(0,hm_finger.bone[0].length/2,0);  // Vi la elipsoid nen dich tinh tien ve tam elipse
		drawElipse(hm_finger.bone[0].radius1,
			hm_finger.bone[0].length/2 + hm_finger.bone[0].radius2,
			hm_finger.bone[0].radius1);          // Ve elipse
		glTranslatef(0,hm_finger.bone[0].length/2,0);
		drawSphere(hm_finger.bone[0].radius2);
		break;

	case HM_FINGER_1:
		offset = OFFSET_FINGER1;
		drawSphere(hm_finger.bone[0].radius1);
		glRotated(handDOFs[offset],0,0,1);
		drawHandBone(hm_finger.bone[0],handDOFs[offset+1]);
		break;

	case HM_FINGER_2:
		offset = OFFSET_FINGER2;
		drawSphere(hm_finger.bone[0].radius1);
		glRotated(handDOFs[offset],0,0,1);
		drawHandBone(hm_finger.bone[0],handDOFs[offset+1]);
		break;
	case HM_FINGER_3:
		offset = OFFSET_FINGER3;
		drawSphere(hm_finger.bone[0].radius1);
		glRotated(handDOFs[offset],0,0,1);
		drawHandBone(hm_finger.bone[0],handDOFs[offset+1]);
		break;

	case HM_FINGER_LITTLE:
		offset = OFFSET_LITTLE;
		drawSphere(hm_finger.bone[0].radius1);
		glRotated(handDOFs[offset],0,0,1);
		drawHandBone(hm_finger.bone[0],handDOFs[offset+1]);
		break;

	}
	glPushMatrix();
	if (offset == OFFSET_THUMB)	glRotatef(45,0,1,0);
	drawHandBone(hm_finger.bone[1],handDOFs[offset+2]);
		glPushMatrix();  // Lai dich goc ve toi vi tri dot 2
			drawHandBone(hm_finger.bone[2],handDOFs[offset+3]);  // Ve dot cuoi cung
		glPopMatrix();  // Quay lai goc ve dot 2
	glPopMatrix();
}


// Ve long ban tay
void HandModel::drawPalm(float width,float height,float depth)
{
	float musculeSize = finger_thumb.bone[0].radius2;
	glTranslatef(width/10,depth/2,0);  // dich toi tam long ban tay

	glPushMatrix();  // luu lai vi tri tam ve
	glTranslatef(0,0,musculeSize - depth/2);
	drawElipse(width/2.0f,musculeSize*1.5f,musculeSize);  // Ve elipse tren
	glPopMatrix();

	glPushMatrix();
	drawElipCylinder(width/2,depth/2,height);  // Ve hinh tru (mau vang)
	glPopMatrix(); 

	glPushMatrix();
	glTranslatef(0,height,musculeSize - depth/2);
	drawElipse(width/2,musculeSize*1.5f,musculeSize);  // Ve elipsoid duoi
	glPopMatrix();
}

void HandModel::drawHand()
{
	glScalef(handScale,handScale,handScale);// scale from mm to pixel
	glTranslated(wirst_position[0],wirst_position[1],wirst_position[2]);
	glPushMatrix();
	
	glRotated(handDOFs[0], 1.f, 0.f, 0.f);  
	glRotated(handDOFs[1], 0.f, 1.f, 0.f);
	glRotated(handDOFs[2], 0.f, 0.f, 1.f);
	
	drawPalmAndFiveFingers();// draw hand data
	glPopMatrix();
}

// Ve ca ban tay
void HandModel::drawPalmAndFiveFingers()
{
	 /*Vi tri con tro o co tay
	thumb*/
	glPushMatrix();  // Luu lai vi tri con tro
	glTranslatef(-20,10,0);  // Dich toi vi tri dot 1 ngon cai
	drawFinger(finger_thumb);  // Ve ngon cai
	glPopMatrix();

	//other 4 fingers
	float root1[3] = {	-20.f,		75.f,		0.f} ;
	float root2[3] = {	0.f,		75.f,		0.f} ;
	float root3[3] = {	20.f,		75.f,		0.f} ;
	float root4[3] = {	38.f,		70.f,		0.f} ;//little finger

	glPushMatrix();
	drawPalm(80,60,20);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(root1[0],root1[1],root1[2]);
	drawFinger(finger_1);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(root2[0],root2[1],root2[2]);
	drawFinger(finger_2);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(root3[0],root3[1],root3[2]);
	drawFinger(finger_3);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(root4[0],root4[1],root4[2]);
	drawFinger(finger_little);
	glPopMatrix();

	frame++;

	if(frame % 5000 == 0 )
	{
		double currentTime = glfwGetTime();
		printf("FPS = %f\n",frame/(currentTime - lastTime));
	}
}

void HandModel::generateSphereBuffer()
{
	GLfloat radius = 1;
	GLint slices = N_SLICES;
	GLint stacks = N_STACKS;
	int num_vertices = (slices+2)*2 + (stacks-2)*(slices+1)*2;
	GLfloat * vertices = new GLfloat[num_vertices*3];

	createSphereVertices(vertices,radius,slices,stacks);
	
	// generate a new VBO
	glGenBuffersARB(1, &sphereVertexBuf);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, sphereVertexBuf);
	glBufferDataARB(GL_ARRAY_BUFFER_ARB, num_vertices*3*sizeof(float), vertices, GL_STATIC_DRAW_ARB);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	// generate a new DrawList
	sphereDisplayList = glGenLists(1);
	glNewList(sphereDisplayList, GL_COMPILE);
	
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, sphereVertexBuf);
	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer(3, GL_FLOAT, 0, 0);
	glDrawArrays(GL_TRIANGLE_FAN, 0, N_SLICES+2);
	glDrawArrays(GL_QUAD_STRIP, N_SLICES+2,500);
	glDrawArrays(GL_TRIANGLE_FAN, 526,N_SLICES+2);
	glDisableClientState(GL_VERTEX_ARRAY);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	glEndList();
}

void HandModel::drawSphere(GLfloat radius)
{
	glColor3f(0,1,0);
	glPushMatrix();
	glScalef(radius,radius,radius);
	glCallList(sphereDisplayList);
	glPopMatrix();
}

void HandModel::drawElipse(GLfloat radiusX,GLfloat radiusY,GLfloat radiusZ)
{
	glColor3f(1,0,0);
	glPushMatrix();
	glScalef(radiusX,radiusY,radiusZ);
	glCallList(sphereDisplayList);
	glPopMatrix();
}


void HandModel::generateCylinderBuffer()
{
	GLfloat radius=1;
	GLint slices = N_SLICES;
	int num_vertices = (slices+2)*2 + (slices+1)*2;
	GLfloat * vertices = new GLfloat[num_vertices*3];

	createCyliderVertices(vertices,radius,slices);

	glGenBuffersARB(1, &cylinderVertexBuffer);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, cylinderVertexBuffer);
	glBufferDataARB(GL_ARRAY_BUFFER_ARB, num_vertices*3*sizeof(float), vertices, GL_STATIC_DRAW_ARB);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	cylinderDisplayList = glGenLists(1);
	glNewList(cylinderDisplayList, GL_COMPILE);

	glBindBufferARB(GL_ARRAY_BUFFER_ARB, cylinderVertexBuffer);
	glEnableClientState(GL_VERTEX_ARRAY);
	glVertexPointer(3, GL_FLOAT, 0, 0);
	glDrawArrays(GL_TRIANGLE_FAN, 0, N_SLICES+2);
	glDrawArrays(GL_TRIANGLE_FAN, N_SLICES+2,N_SLICES+2);
	glDrawArrays(GL_QUAD_STRIP, (N_SLICES+2)*2,2*N_SLICES+2);
	glDisableClientState(GL_VERTEX_ARRAY);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

	glEndList();
}

void HandModel::drawCylinder(GLfloat radius,GLfloat height)
{
	glColor3f(0,0,1);
	glPushMatrix();
	glScalef(radius,height,radius);
	glCallList(cylinderDisplayList);
	glPopMatrix();
}

void HandModel::drawElipCylinder(GLfloat radiusX,GLfloat radiusZ,GLfloat height)
{
	glColor3f(1,1,0);
	glPushMatrix();
	glScalef(radiusX,height,radiusZ);
	glCallList(cylinderDisplayList);
	glPopMatrix();
}

void HandModel::generateConeBuffer(GLuint & buf,GLfloat botRadius,GLfloat topRadius,GLfloat height)
{
	GLint slices = N_SLICES;
	int num_vertices = (slices+1)*2;
	GLfloat * vertices = new GLfloat[num_vertices*3];

	createConeVertices(vertices,botRadius,topRadius,height,slices);

	glGenBuffersARB(1, &buf);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, buf);
	glBufferDataARB(GL_ARRAY_BUFFER_ARB, num_vertices*3*sizeof(float), vertices, GL_STATIC_DRAW_ARB);
	glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);
}

void HandModel::generateFingersBuffer()
{
	generateConeBuffer(finger_thumb.bone[1].vertexBuf,
		finger_thumb.bone[1].radius1,
		finger_thumb.bone[1].radius2,
		finger_thumb.bone[1].length);
	createDisplayList(finger_thumb.bone[1].vertexBuf,
		finger_thumb.bone[1].vertexDisplayList);

	generateConeBuffer(finger_thumb.bone[2].vertexBuf,
		finger_thumb.bone[2].radius1,
		finger_thumb.bone[2].radius2,
		finger_thumb.bone[2].length);
	createDisplayList(finger_thumb.bone[2].vertexBuf,
		finger_thumb.bone[2].vertexDisplayList);

	for (int i=0;i<3;i++)
	{
		generateConeBuffer(finger_1.bone[i].vertexBuf,finger_1.bone[i].radius1,
			finger_1.bone[i].radius2,finger_1.bone[i].length);
		createDisplayList(finger_1.bone[i].vertexBuf,
			finger_1.bone[i].vertexDisplayList);

		generateConeBuffer(finger_2.bone[i].vertexBuf,
			finger_2.bone[i].radius1,
			finger_2.bone[i].radius2,
			finger_2.bone[i].length);
		createDisplayList(finger_2.bone[i].vertexBuf,
			finger_2.bone[i].vertexDisplayList);

		generateConeBuffer(finger_3.bone[i].vertexBuf,
			finger_3.bone[i].radius1,
			finger_3.bone[i].radius2,
			finger_3.bone[i].length);
		createDisplayList(finger_3.bone[i].vertexBuf,
			finger_3.bone[i].vertexDisplayList);

		generateConeBuffer(finger_little.bone[i].vertexBuf,
			finger_little.bone[i].radius1,
			finger_little.bone[i].radius2,
			finger_little.bone[i].length);
		createDisplayList(finger_little.bone[i].vertexBuf,
			finger_little.bone[i].vertexDisplayList);
	}
}